import vue from '../main'
import { Notification  } from 'element-ui';
import { OBJ } from './init'
import { PARAMS } from "./params"

/**
 * 急停
 */
export function stopRobot() {
    manualCtrl({
        linear: 0,
        angular: 0
    })
    navCtrl('', 0);
    Notification ({
        title: vue.$t("titles.success"),
        message: vue.$t("messages.taskCanceled"),
        type: 'success',
        position: 'top-left',
        offset: 100
    });
};

/**
 * 导航到指定站点
 * **/
export function navNewStation(fullName) {
    navCtrl(fullName, 1);
};

/**
 * 导航
 * 在0或4空闲状态
 * 不允许发送任务
 * control：0是停止，1是启动
 * **/
export function navCtrl(name, control, ...rest) {
    var allowInterrupt = false;
    if (rest[0] === true) {
        allowInterrupt = true;
    }
    console.log(`[INFO] navstatus ${OBJ.navCtrlStatus.status}`)
    if (!allowInterrupt) {
        if (OBJ.navCtrlStatus.status !== PARAMS.NavCtrlStatus.idling && OBJ.navCtrlStatus.status !== PARAMS.NavCtrlStatus.cancelled) {
            if (control === PARAMS.NavCtrl.start) {
                Notification({
                    title: vue.$t("titles.warning"),
                    message: vue.$t("messages.tasksBeing"),
                    type: 'warning',
                    position: 'top-left',
                    offset: 100
                });                  
                return;
            }
        }
    }
    var msg = new ROSLIB.Message({
        goal_name: name,
        control: control
    });
    OBJ.topic.navCtrlTopic.publish(msg);
}


/**
 * 手柄操作导航开始
 * type: up/right/bottom/left  上/下/左/右
 * **/
export function naigationStart(type){
    switch(type){
        case "up":
            PARAMS.joystickObj.up = true;
            if(PARAMS.joystickObj.up && PARAMS.joystickObj.down) {
                manualCtrl({
                    linear: 0,
                    angular: 0
                })
                return
            };
            PARAMS.joystickObj.upTimeout = setInterval(function () {
                PARAMS.joystickObj.upTime += 0.05;
                if(PARAMS.joystickObj.upTime >= PARAMS.joystickObj.maxSpeed){
                    PARAMS.joystickObj.upTime = PARAMS.joystickObj.maxSpeed;
                }
                if (PARAMS.joystickObj.left){
                    manualCtrl({linear: PARAMS.joystickObj.upTime, angular: PARAMS.joystickObj.leftTime})
                    //上，左
                }else if(PARAMS.joystickObj.right){
                    manualCtrl({linear: PARAMS.joystickObj.upTime, angular: -PARAMS.joystickObj.rightTime})
                    //上，右
                }else{
                    //上
                    manualCtrl({linear: PARAMS.joystickObj.upTime})
                }
            }, 400)
            break;
        case "down":
            PARAMS.joystickObj.down = true;
            if(PARAMS.joystickObj.up && PARAMS.joystickObj.down) {
                manualCtrl({
                    linear: 0,
                    angular: 0
                })
                return
            };
            PARAMS.joystickObj.downTimeout = setInterval(function () {
                PARAMS.joystickObj.downTime += 0.05;
                if(PARAMS.joystickObj.downTime >= PARAMS.joystickObj.maxSpeed){
                    PARAMS.joystickObj.downTime = PARAMS.joystickObj.maxSpeed;
                }
                if (PARAMS.joystickObj.left){
                    manualCtrl({linear: -PARAMS.joystickObj.downTime, angular: PARAMS.joystickObj.leftTime})
                    //上，左
                }else if(PARAMS.joystickObj.right){
                    manualCtrl({linear: -PARAMS.joystickObj.downTime, angular: -PARAMS.joystickObj.rightTime})
                    //上，右
                }else{
                    //上
                    manualCtrl({linear: -PARAMS.joystickObj.downTime})
                }
            }, 400)
            break;
        case "left":
            PARAMS.joystickObj.left = true;
            if(PARAMS.joystickObj.left && PARAMS.joystickObj.right) {
                manualCtrl({
                    linear: 0,
                    angular: 0
                })
                return
            };
            PARAMS.joystickObj.leftTimeout = setInterval(function () {
                PARAMS.joystickObj.leftTime += 0.05;
                if(PARAMS.joystickObj.leftTime >= PARAMS.joystickObj.maxSpeed){
                    PARAMS.joystickObj.leftTime = PARAMS.joystickObj.maxSpeed;
                }
                if (PARAMS.joystickObj.up){
                    manualCtrl({linear: PARAMS.joystickObj.upTime, angular: PARAMS.joystickObj.leftTime});
                    //上，左
                }else if(PARAMS.joystickObj.down){
                    manualCtrl({linear: -PARAMS.joystickObj.downTime, angular: PARAMS.joystickObj.leftTime});
                    //上，右
                }else{
                    //上
                    manualCtrl({angular: PARAMS.joystickObj.leftTime});
                }
            }, 400)
            break;
        case "right":
            PARAMS.joystickObj.right = true;
            if(PARAMS.joystickObj.left && PARAMS.joystickObj.right) {
                manualCtrl({
                    linear: 0,
                    angular: 0
                })
                return
            };
            PARAMS.joystickObj.rightTimeout = setInterval(function () {
                PARAMS.joystickObj.rightTime += 0.05;
                if(PARAMS.joystickObj.rightTime >= PARAMS.joystickObj.maxSpeed){
                    PARAMS.joystickObj.rightTime = PARAMS.joystickObj.maxSpeed;
                }
                if (PARAMS.joystickObj.up){
                    manualCtrl({linear: PARAMS.joystickObj.upTime, angular: -PARAMS.joystickObj.rightTime})
                    //上，左
                }else if(PARAMS.joystickObj.down){
                    manualCtrl({linear: -PARAMS.joystickObj.downTime, angular: -PARAMS.joystickObj.rightTime})
                    //上，右
                }else{
                    //上
                    manualCtrl({angular: -PARAMS.joystickObj.rightTime})
                }
            }, 400)
            break;
        default:
            break;
    }
}

/**
 * 手柄操作导航结束
 * type:  up/right/bottom/left  上/下/左/右
 * **/
export function navigationEnd(type){
    switch(type){
        case "up":
            PARAMS.joystickObj.upTime = 0;
            PARAMS.joystickObj.up = false;
            clearInterval(PARAMS.joystickObj.upTimeout);
            manualCtrl({linear: PARAMS.joystickObj.upTime})
            break;
        case "down":
            PARAMS.joystickObj.downTime = 0;
            PARAMS.joystickObj.down = false;
            clearInterval(PARAMS.joystickObj.downTimeout);
            manualCtrl({linear: PARAMS.joystickObj.downTime})
            break;
        case "left":
            PARAMS.joystickObj.leftTime = 0;
            PARAMS.joystickObj.left = false;
            clearInterval(PARAMS.joystickObj.leftTimeout);
            manualCtrl({linear: PARAMS.joystickObj.leftTime})
            break;
        case "right":
            PARAMS.joystickObj.rightTime = 0;
            PARAMS.joystickObj.right = false;
            clearInterval(PARAMS.joystickObj.rightTimeout);
            manualCtrl({linear: PARAMS.joystickObj.rightTime})
            break;
        default:
            break
    }
}
/**
 * 控制小车移动方向
 * 0：停止
 * **/
export function manualCtrl(info) {
    let linear_ = info.linear || 0;
    let angular_ = info.angular ||0;
    console.log(`linear_:${linear_}`)
    console.log(`angular_:${angular_}`)
    var msg = new ROSLIB.Message({
        linear: {
            x: linear_,
            y: 0,
            z: 0
        },
        angular: {
            x: 0,
            y: 0,
            z: angular_
        }
    });
    OBJ.topic.cmdVelTopic.publish(msg);
}

/**
 * 判断导航状态是否正在执行其他任务
 * **/
export function verifyNavCtrlStatus() {
    if (OBJ.navCtrlStatus.status !== PARAMS.NavCtrlStatus.idling && OBJ.navCtrlStatus.status !== PARAMS.NavCtrlStatus.cancelled) {
        Notification({
            title: vue.$t("titles.warning"),
            message: vue.$t("messages.tasksBeing"),
            type: 'warning',
            position: 'top-left',
            offset: 100
        });
        return false;
    }
    return true;
}